#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;}
void InitWii(void);
void ReadWii(uint8 *buf);
void CalibrateGyros(void);
void CalibrateAcc(void);
 void initRam(void)
{
  firstSample=true;
  gyroRollAngle=0;
  gyroPitchAngle=0;
  acc_1G=(4096.0*0.6)/3.3;
}
void InitWii(void) {
i2c_msg msgs[1];
uint8 buf[2] = {0xf0, 0x55};
pinMode(45, OUTPUT);

  digitalWrite(45, LOW);    // sets the LED off
  delay(200);                         // waits for a second
 i2c_master_enable(I2C2, 0);
 delay(100);
 msgs[0].addr = 0x53;
 msgs[0].flags = 0;
 msgs[0].length = 2;
 msgs[0].data = buf;
 i2c_master_xfer(I2C2, msgs, 1,0);
 delay(100);
 buf[0] = 0xFe; buf[1] = 0x04;
 msgs[0].addr = 0x53;
 msgs[0].flags = 0;
 msgs[0].length = 2;
 msgs[0].data = buf;
  i2c_master_xfer(I2C2, msgs, 1,0);
 delay(100);
 initRam();
 CalibrateGyros();
 CalibrateAcc();
}

void ReadWii(uint8 *buf) {
i2c_msg msgs[2];

 buf[0] = 0x00;
 msgs[0].addr = 0x52;
 msgs[0].flags = 0;
 msgs[0].length = 1;
 msgs[0].data = buf;

 /* Repeated start condition, then read 6 byte back */
 msgs[1].addr = 0x52;
 msgs[1].flags = I2C_MSG_READ;
 msgs[1].length = 6;
 msgs[1].data = buf;
 i2c_master_xfer(I2C2, msgs, 2,0);
}
void CalibrateAcc(void)
{
  unsigned int i;
  for(i=0;i<32;i++)
  { 
    xValueZero+=analogRead(xPin);
    yValueZero+=analogRead(yPin);
    zValueZero+=analogRead(zPin);
     toggleLED();
  delay(30);
  }
   xValueZero=(xValueZero>>5);
   yValueZero=(yValueZero>>5);
   zValueZero=(zValueZero>>5);
   zValueZero=(xValueZero+yValueZero)/2;
}
void CalibrateGyros(void)
{
unsigned int i;
uint8 buffer[6];

 for (i=0;i<32;i++){  // Average of 32 readings of gyros
  ReadWii(buffer);
  gyroADC[YAW]=   ((buffer[3] >> 2) << 8) + buffer[0];
  gyroADC[PITCH] = ((buffer[4] >> 2) << 8) + buffer[1];
  gyroADC[ROLL] =  ((buffer[5] >> 2) << 8) + buffer[2];
  PitchGyroZero  +=  gyroADC[PITCH];
  RollGyroZero   += gyroADC[ROLL] ;
  YawGyroZero    += gyroADC[YAW];
  toggleLED();
  delay(30);
 }
 PitchGyroZero = (PitchGyroZero >> 5);
 RollGyroZero  = (RollGyroZero  >> 5);
 YawGyroZero   = (YawGyroZero   >> 5);
 
}


float accelScaleFactor = ((3.3/4096.0/0.6)* 9.80665);
 

void updateAccData(void)
{
  int i;
   accADC[0] = (analogRead(xPin)-xValueZero);
     #ifdef FlightAngle_ARG_MODE //DCM与互补 方向不一致
   accADC[1]= -(analogRead(yPin)-yValueZero);
    accADC[2]= -(analogRead(zPin)-zValueZero);
   #else
   accADC[1]= (analogRead(yPin)-yValueZero);
    accADC[2]= (analogRead(zPin)-zValueZero);
   #endif
   
 
  
      xValue = filterSmooth( accADC[0]*accelScaleFactor   ,xValue, 0.2); //LFC
      yValue = filterSmooth( accADC[1]*accelScaleFactor   ,yValue, 0.2); //LFC
      zValue = filterSmooth( accADC[2]*accelScaleFactor  ,zValue, 0.2); //LFC
         
    
    if(abs(zValue)<0.1)
    {
      accVector[0]=0;
      accVector[1]=0;
      accVector[2]=1;
     // RwAcc[0]=0;
     // RwAcc[1]=0;
     // RwAcc[2]=1;
    }
    else 
    {
      accVector[0]=xValue;
     accVector[1]=yValue;
     accVector[2]=zValue;
    
      RwAcc[0]=xValue;
      RwAcc[1]=yValue;
      RwAcc[2]=zValue;
    }
    normalize3DVector(RwAcc);
     normalize3DVector(accVector);
    accXZAngle=degrees(atan2(RwAcc[0],RwAcc[2]) );//* 180 / PI;
   accYZAngle=degrees(atan2(RwAcc[1],RwAcc[2]));// * 180 / PI;
    
   
}

 
#ifdef FlightAngle_ARG_MODE
float wmpLowRangeToRadPerSec  = 0.001082308;
float wmpHighRangeToRadPerSec = 0.005844461;//弧度
#else
float wmpLowRangeToRadPerSec =0.06201166;
float wmpHighRangeToRadPerSec =0.33486295;//度
#endif
 float gyroScaleFactor;
 
void updateGyroData(void)
{
   uint8 buffer[6]; 
   ReadWii(buffer);
         if (buffer[5] & 0x02) //If WiiMP
         {

             gyroADC[ROLL] =   (((buffer[5] >> 2) << 8) + buffer[2])-RollGyroZero ;//x，，ROLL是绕着X转圈即变方向
           
            if(buffer[4]&0x02)gyroScaleFactor=wmpLowRangeToRadPerSec;
              else gyroScaleFactor=wmpHighRangeToRadPerSec;
              
              RollGyroValue = filterSmooth( gyroADC[ROLL]  * gyroScaleFactor,RollGyroValue, 0.2); 
             #ifdef FlightAngle_ARG_MODE 
             gyroADC[PITCH] =((((buffer[4] >> 2) << 8) + buffer[1])-PitchGyroZero);//y pitch是绕着y转圈
             #else 
             gyroADC[PITCH] =-((((buffer[4] >> 2) << 8) + buffer[1])-PitchGyroZero);//y pitch是绕着y转圈
              #endif
              if(buffer[3]&0x01)gyroScaleFactor=wmpLowRangeToRadPerSec;
              else gyroScaleFactor=wmpHighRangeToRadPerSec;
              PitchGyroValue=filterSmooth( gyroADC[PITCH]  * gyroScaleFactor,PitchGyroValue,0.2); 
              
              gyroADC[YAW] = YawGyroZero-(((buffer[3] >> 2) << 8) + buffer[0] ); //z YAW 是绕Z轴转
              if(buffer[3]&0x02)gyroScaleFactor=wmpLowRangeToRadPerSec;
              else gyroScaleFactor=wmpHighRangeToRadPerSec;
             
              YawGyroValue=filterSmooth( gyroADC[YAW]  * gyroScaleFactor,YawGyroValue, 0.2); 
          
   Gyro_ds[0] = RollGyroValue;
      Gyro_ds[1] = PitchGyroValue;
   Gyro_ds[2] = YawGyroValue;
         }
}
void normalize3DVector(float* vector)//各个投影分量
{
  static float R;  
  R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
  vector[0] /= R;
  vector[1] /= R;  
  vector[2] /= R;  
}
float squared(float x){
   return x*x;
}
void getInclination() {
 int w = 0;
 float tmpf = 0.0;
 int currentTime1, signRzGyro;

   currentTime1 = millis();
   interval = currentTime1 - lastTime;
   lastTime = currentTime1;

   if (firstSample) { // the NaN check is used to wait for good data from the Arduino
    for(w=0;w<=2;w++) {
      RwEst[w] = RwAcc[w];    //initialize with accelerometer readings
    }
    gyroRollAngle=0;
    gyroPitchAngle=0;
   }
   else{
    //evaluate RwGyro vector
    if(abs(RwEst[2]) < 0.1) {
      //Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading to bad results
      //in this case skip the gyro data and just use previous estimate
      for(w=0;w<=2;w++) {
        RwGyro[w] = RwEst[w];
      }
    }
    else {
      //get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
      for(w=0;w<=1;w++){
        tmpf = Gyro_ds[w];                        //get current gyro rate in deg/s
        tmpf *= G_Dt;//interval / 1000.0f;                     //get angle change in deg
        Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI;   //get angle and convert to degrees
        Awz[w] += tmpf;             //get updated angle according to gyro movement
        if(w==0)//chwh add debug
       {
         gyroRollAngle+=tmpf;
          
       }
       else gyroPitchAngle+=tmpf;
      }

      //estimate sign of RzGyro by looking in what qudrant the angle Axz is,
      //RzGyro is pozitive if  Axz in range -90 ..90 => cos(Awz) >= 0
      signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;

      //reverse calculation of RwGyro from Awz angles, for formulas deductions see  http://starlino.com/imu_guide.html
      for(w=0;w<=1;w++){
        RwGyro[0] = sin(Awz[0] * PI / 180);
        RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
        RwGyro[1] = sin(Awz[1] * PI / 180);
        RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
      }
      RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
    }

    //combine Accelerometer and gyro readings
    for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);

    normalize3DVector(RwEst);
  }

  firstSample = false;
}
